OctoMapPointStamped[] mapPoints
